#include "pointcloud_fusion_core.h"

PointCloudFusion::PointCloudFusion():nh_(""), private_nh_("~"), listener_(buffer_){
    private_nh_.param<std::string>("input_lidar_topic_", input_lidar_topic_, "");
    private_nh_.param<std::string>("input_lidar_frame_id", input_lidar_frame_id, "");
    private_nh_.param<std::string>("input_map_border_topic_", input_map_border_topic_, "");
    private_nh_.param<std::string>("input_map_border_frame_id", input_map_border_frame_id, "");
    private_nh_.param<std::string>("output_confusion_lidar_topic_", output_confusion_lidar_topic_, "");
    private_nh_.param<std::string>("output_confusion_lidar_frame_id", output_confusion_lidar_frame_id, "");

    private_nh_.param<double>("passthrough_x_min", passthrough_x_min, 0.0);
    private_nh_.param<double>("passthrough_x_max", passthrough_x_max, 0.0);
    private_nh_.param<double>("passthrough_y_min", passthrough_y_min, 0.0);
    private_nh_.param<double>("passthrough_y_max", passthrough_y_max, 0.0);
    private_nh_.param<double>("passthrough_z_min", passthrough_z_min, 0.0);
    private_nh_.param<double>("passthrough_z_max", passthrough_z_max, 0.0);

    private_nh_.param<double>("map_border_passthrough_x_min", map_border_passthrough_x_min, 0.0);
    private_nh_.param<double>("map_border_passthrough_x_max", map_border_passthrough_x_max, 0.0);
    private_nh_.param<double>("map_border_passthrough_y_min", map_border_passthrough_y_min, 0.0);
    private_nh_.param<double>("map_border_passthrough_y_max", map_border_passthrough_y_max, 0.0);
}

PointCloudFusion::~PointCloudFusion(){}

void PointCloudFusion::run(){
    lidar_sub_ = nh_.subscribe<sensor_msgs::PointCloud2>(input_lidar_topic_, 1, &PointCloudFusion::lidar_call_back, this);
    map_border_sub_ = nh_.subscribe<sensor_msgs::PointCloud2>(input_map_border_topic_, 1, &PointCloudFusion::map_border_call_back, this);
    confusion_lidar_pub_ = nh_.advertise<sensor_msgs::PointCloud2>(output_confusion_lidar_topic_, 1);
    ros::Timer timer = nh_.createTimer(ros::Duration(0.1), &PointCloudFusion::process, this, false, true);
    ros::spin();
}

void PointCloudFusion::map_border_call_back(const sensor_msgs::PointCloud2::ConstPtr& msg){
    try{
        geometry_msgs::TransformStamped transform_stamed = buffer_.lookupTransform(output_confusion_lidar_frame_id, input_map_border_frame_id, ros::Time(0));
        geometry_msgs::Transform transform;
        transform.translation = transform_stamed.transform.translation;
        transform.rotation = transform_stamed.transform.rotation;

        Eigen::Matrix4f eigen_transform;
        pcl_ros::transformAsMatrix(transform, eigen_transform);
        pcl_ros::transformPointCloud(eigen_transform, *msg, map_border_pc_trans);
        map_border_pc_trans.header.frame_id = output_confusion_lidar_frame_id;
    }catch (tf2::TransformException &ex)
    {
        ROS_WARN("[%s] map border lookupTransform error, %s", _APP_NAME_, ex.what());
    }
}

void PointCloudFusion::lidar_call_back(const sensor_msgs::PointCloud2::ConstPtr& msg){
    try{
        geometry_msgs::TransformStamped transform_stamed = buffer_.lookupTransform(output_confusion_lidar_frame_id, input_lidar_frame_id, ros::Time(0));
        geometry_msgs::Transform transform;
        transform.translation = transform_stamed.transform.translation;
        transform.rotation = transform_stamed.transform.rotation;

        Eigen::Matrix4f eigen_transform;
        pcl_ros::transformAsMatrix(transform, eigen_transform);
        pcl_ros::transformPointCloud(eigen_transform, *msg, lidar_pc_trans);
        lidar_pc_trans.header.frame_id = output_confusion_lidar_frame_id;
    }catch (tf2::TransformException &ex)
    {
        ROS_WARN("[%s] left lidar lookupTransform error, %s", _APP_NAME_, ex.what());
    }
}


void PointCloudFusion::process(const ros::TimerEvent& e){
    pcl::PointCloud<pcl::PointXYZ>::Ptr pcl_confusion_pc(new pcl::PointCloud<pcl::PointXYZ>());
    pcl::PointCloud<pcl::PointXYZ>::Ptr pcl_map_border_pc(new pcl::PointCloud<pcl::PointXYZ>());
    if(fabs(ros::Time().now().toSec()-lidar_pc_trans.header.stamp.toSec())<0.5){
        pcl::PointCloud<pcl::PointXYZ>::Ptr lidar_pc2(new pcl::PointCloud<pcl::PointXYZ>());
        pcl::fromROSMsg(lidar_pc_trans, *lidar_pc2);
        *pcl_confusion_pc = *pcl_confusion_pc + *lidar_pc2;
    }else{
        ROS_WARN("[%s] velodyne lidar timeout", _APP_NAME_);
    }


    if(fabs(ros::Time().now().toSec()-map_border_pc_trans.header.stamp.toSec())<0.5){
        pcl::fromROSMsg(map_border_pc_trans, *pcl_map_border_pc);
    }else{
        // ROS_WARN("[%s] map border timeout", _APP_NAME_);
    }

    /* 车体滤波 */
    pcl::PassThrough<pcl::PointXYZ> pt;
    pcl::PointCloud<pcl::PointXYZ>::Ptr pcl_pc_outter_x(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::PointCloud<pcl::PointXYZ>::Ptr pcl_pc_inner_x(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::PointCloud<pcl::PointXYZ>::Ptr pcl_pc_outter_y(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::PointCloud<pcl::PointXYZ>::Ptr pcl_pc_outter_xy(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::PointCloud<pcl::PointXYZ>::Ptr pcl_pc_inner_z(new pcl::PointCloud<pcl::PointXYZ>);

    pt.setInputCloud(pcl_confusion_pc);
    pt.setFilterFieldName("x");
    pt.setFilterLimits(passthrough_x_min, passthrough_x_max);
    pt.setNegative(false);
    pt.filter(*pcl_pc_inner_x);

    pt.setInputCloud(pcl_confusion_pc);
    pt.setFilterFieldName("x");
    pt.setFilterLimits(passthrough_x_min, passthrough_x_max);
    pt.setNegative(true);
    pt.filter(*pcl_pc_outter_x);

    pt.setInputCloud(pcl_pc_inner_x);
    pt.setFilterFieldName("y");
    pt.setFilterLimits(passthrough_y_min, passthrough_y_max);
    pt.setNegative(true);
    pt.filter(*pcl_pc_outter_y);
    
    *pcl_pc_outter_xy = *pcl_pc_outter_x + *pcl_pc_outter_y;

    pt.setInputCloud(pcl_pc_outter_xy);
    pt.setFilterFieldName("z");
    pt.setFilterLimits(passthrough_z_min, passthrough_z_max);
    pt.setNegative(false);
    pt.filter(*pcl_pc_inner_z);

    /* 地图边界滤波 */
    pt.setInputCloud(pcl_map_border_pc);
    pt.setFilterFieldName("x");
    pt.setFilterLimits(map_border_passthrough_x_min, map_border_passthrough_x_max);
    pt.filter(*pcl_map_border_pc);
    pt.setInputCloud(pcl_map_border_pc);
    pt.setFilterFieldName("y");
    pt.setFilterLimits(map_border_passthrough_y_min, map_border_passthrough_y_max);
    pt.filter(*pcl_map_border_pc);

    /* 叠加map_border */
    *pcl_pc_inner_z = *pcl_pc_inner_z + *pcl_map_border_pc;

    sensor_msgs::PointCloud2 ros_pc2_out;
    pcl::toROSMsg(*pcl_pc_inner_z, ros_pc2_out);
    ros_pc2_out.header.stamp = ros::Time::now();
    ros_pc2_out.header.frame_id = output_confusion_lidar_frame_id;
    confusion_lidar_pub_.publish(ros_pc2_out);
}
